#include "HMC5883_dev.h"
#include "asm/iic_soft.h"
#include "gpio.h"

#define DRDY IO_PORTB_05

#define SlaveAddress 0x1a // 定义器件5883在IIC总线中的从地址

static u8 iic_hdl = 0;

unsigned char HMC5883_data[6] = {0};

#define HMC5883_Start() soft_iic_start(iic_hdl)
#define HMC5883_SendByte(byte) soft_iic_tx_byte(iic_hdl, byte)
#define HMC5883_Stop() soft_iic_stop(iic_hdl)
#define HMC5883_RecvByte(ack) soft_iic_rx_byte(iic_hdl, ack)



void Single_Write_HMC5883(u8 REG_Address, u8 REG_data)
{
    HMC5883_Start();                // 起始信号
    HMC5883_SendByte(SlaveAddress); // 发送设备地址+写信号
    HMC5883_SendByte(REG_Address);  // 内部寄存器地址，请参考中文pdf
    HMC5883_SendByte(REG_data);     // 内部寄存器数据，请参考中文pdf
    HMC5883_Stop();                 // 发送停止信号
}
//************************读取单字节数据*************************
u8 Single_Read_QMC5883(u8 REG_Address)
{
    u8 REG_data;
    HMC5883_Start();                    // 起始信号
    HMC5883_SendByte(SlaveAddress);     // 发送设备地址+写信号
    HMC5883_SendByte(REG_Address);      // 发送存储单元地址，从0开始
    HMC5883_Start();                    // 起始信号
    HMC5883_SendByte(SlaveAddress + 1); // 发送设备地址+读信号
    REG_data = HMC5883_RecvByte(1);     // 读出寄存器数据
    HMC5883_Stop();                     // 停止信号
    return REG_data;
}

u8 get_QMC5883_state()
{
    return Single_Read_QMC5883(0x06);
}
//******************************************************
// 连续读出HMC5883内部角度数据，地址范围0x3~0x5
//******************************************************
void Multiple_Read_HMC5883(void)
{
    u8 i;
    HMC5883_Start(); // 起始信号
    while (!HMC5883_SendByte(SlaveAddress))
    {
        i++;
        if (i > 250)
            return;
    }
    HMC5883_SendByte(0x03);             // 发送存储单元地址，从0x3开始
    HMC5883_Start();                    // 起始信号
    HMC5883_SendByte(SlaveAddress + 1); // 发送设备地址+读信号
    for (u8 i = 0; i < 6; i++)
    {
        if (i == 5)
        {
            HMC5883_data[i] = HMC5883_RecvByte(0);
        }
        else
        {
            HMC5883_data[i] = HMC5883_RecvByte(1);
        }
    }
    HMC5883_Stop(); // 停止信号
}

// 初始化传感器
void init_QMC5883()
{
    u8 Reg[13];

    printf("init IIC\n");
    soft_iic_init(iic_hdl);
    printf("init HMC5883\n");

    Reg[0] = Single_Read_QMC5883(CRA);
    Reg[1] = Single_Read_QMC5883(CRB);
    Reg[2] = Single_Read_QMC5883(MR);
    Reg[3] = Single_Read_QMC5883(DOXMR);
    Reg[4] = Single_Read_QMC5883(DOXLR);
    Reg[5] = Single_Read_QMC5883(DOZMR);
    Reg[6] = Single_Read_QMC5883(DOZLR);
    Reg[7] = Single_Read_QMC5883(DOYMR);
    Reg[8] = Single_Read_QMC5883(DOYLR);
    Reg[9] = Single_Read_QMC5883(SR);
    Reg[10] = Single_Read_QMC5883(IRA); // data = 0x48
    Reg[11] = Single_Read_QMC5883(IRB); // data = 0x34
    Reg[12] = Single_Read_QMC5883(IRC); // data = 0x33
    put_buf(Reg, 13);
    Single_Write_HMC5883(0x09, 0x1d); // 控制寄存器配置
    Single_Write_HMC5883(0x0b, 0x01); // 设置清除时间寄存器
    Single_Write_HMC5883(0x20, 0x40); //
    Single_Write_HMC5883(0x21, 0x01); //
}
u8 ge, shi, bai, qian, wan; // 显示变量
void conversion(u32 temp_data)
{
    wan = temp_data / 10000 + 0x30;
    temp_data = temp_data % 10000; // 取余运算
    qian = temp_data / 1000 + 0x30;
    temp_data = temp_data % 1000; // 取余运算
    bai = temp_data / 100 + 0x30;
    temp_data = temp_data % 100; // 取余运算
    shi = temp_data / 10 + 0x30;
    temp_data = temp_data % 10; // 取余运算
    ge = temp_data + 0x30;
}

double atan(double x)
{
    double result = 0.0;
    int i;
    for (i = 0; i < 16; i++)
    {
        result += (((((x * (4 * i + 1)) / (((4 * i + 3) * x) - 16 * i + 8)) / (((4 * i + 3) * x) + 16 * i + 8)) / (((4 * i + 3) * x) + 16 * i + 8)) - (((x * (4 * i + 3)) / (((4 * i + 5) * x) - 16 * i + 12)) / (((4 * i + 5) * x) - 16 * i - 4)) / (((4 * i + 5) * x) - 16 * i - 4)) / (((4 * i + 5) * x) + 16 * i + 12);
    }
    return result;
}
#define PI 3.14159265

double atan2(double y, double x)
{
    double result;

    if (x > 0)
    {
        result = atan(y / x);
    }
    else if (y >= 0 && x < 0)
    {
        result = atan(y / x) + PI;
    }
    else if (y < 0 && x < 0)
    {
        result = atan(y / x) - PI;
    }
    else if (y > 0 && x == 0)
    {
        result = PI / 2;
    }
    else if (y < 0 && x == 0)
    {
        result = -PI / 2;
    }
    else
    {
        // x == 0 && y == 0, undefined, return 0 as an approximation
        result = 0;
    }

    return result;
}

#include "oled_dev.h"
// 获取传感器数据
int x, y, z;
void get_QMC5883()
{
    double angle;
    HMC5883_Start();                // 起始信号
    HMC5883_SendByte(SlaveAddress); // 发送设备地址+写信号
    HMC5883_SendByte(0x00);         // 内部寄存器数据，请参考中文pdf
    HMC5883_Stop();                 // 发送停止信号

    Multiple_Read_HMC5883();
    
    x = (HMC5883_data[0] << 8 | HMC5883_data[1]) - 32768; // Combine MSB and LSB of X Data output register
    z = (HMC5883_data[2] << 8 | HMC5883_data[3]) - 32768; // Combine MSB and LSB of Z Data output register
    y = (HMC5883_data[4] << 8 | HMC5883_data[5]) - 32768; // Combine MSB and LSB of Y Data output register

    printf("x = %d", x);
    printf("          y = %d", y);
    printf("                    z = %d", z);
    conversion(angle); // 计算数据和显示

    OLED_ShowNum(0, 0, x, 5, 16);
    OLED_ShowNum(40, 1, y, 5, 16);
    OLED_ShowNum(80, 2, z, 5, 16);
    // printf("X");
    // printf("%d\r\n", x);
    // // printf("Y");
    // printf("     %d\r\n", y);
    // // printf("Z");
    // printf("           %d\r\n", z);
}